#include "zeroerr.hpp"
#include <iomanip>
#include <bitset>
#include <chrono>
#include <ctime>
using namespace CANalyst2;

ZeroErr::ZeroErr(std::shared_ptr<CANDispatch> _can, int ch, int _slave_id, bool _is_pos_mode, int _resolution, 
                uint32_t _profiled_vel, uint32_t _profiled_acc, uint32_t _profiled_dec) : 
        is_pos_mode(_is_pos_mode), can(_can), resolution(_resolution), 
        profiled_vel(_profiled_vel), profiled_acc(_profiled_acc), profiled_dec(_profiled_dec)
{
    slave_id = _slave_id;
    can_ch = ch;
    _is_fault = false;
    _error_code = 0;
    _error_req = 0;
    _error_cnt = 0;
    is_motor_on = false;
    memset(&status_fbk, 0, sizeof(status_fbk));

    auto timeout = [this]()
    {
        std::cout << "Kinco servo timeout,addr = " << int(slave_id) << std::endl;
        _is_fault = true;
        _error_code = 0xFFFF;
    };

    can->Register(
        0x180 + slave_id, [this](const struct CanStamped &frame)->void
        { this->processRecvTPDOFrame(frame); },
        200,
        timeout);

    can->Register(
        0x80 + slave_id, [this](const struct CanStamped &frame)->void
        { this->processRecvEmcyFrame(frame); },
        200,
        timeout);

    can->Register(
        0x580 + slave_id, [this](const struct CanStamped &frame)->void
        // {this->updateFault(frame);}
        {},
        200,
        timeout);
}

ZeroErr::~ZeroErr()
{
    std::cout << "~ZeroErr() is called" << std::endl;
    // GetCurStatusPDO();
    // if(current_status != Servo_Status::SWITCH_ON_DISABLED) {
    //     SendDisableMotorPDO();
    // }
    SendSetCWordClrZero();
}

int ZeroErr::SendEnableMotorPDO()
{
    can->clr_send_buffer(can_ch);
    for(int i = 0; i < 10; i++)
    {
        GetCurStatusPDO();
        if(current_status != Servo_Status::INVALID) {
            break;
        }
    }

    if(current_status == Servo_Status::INVALID) {
        std::cout << "Servo motor enable failed, cann't acquire servo motor status-word." << std::endl;
        return -1;
    }
    else if(current_status == Servo_Status::SWITCH_ON_DISABLED) {
        std::cout << "Current status is SWITCH_ON_DISABLED, continue..." << std::endl;
    }

    CanStamped send_frame;
    send_frame.ID = 0x201;
    send_frame.DataLen = 8;
    send_frame.Data[0] = 0x26;
    send_frame.Data[1] = 0x00;
    send_frame.Data[2] = 0x00;
    send_frame.Data[3] = 0x00;
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;

    for(int i = 0; i < 10; i++)
    {
        can->clr_send_buffer(can_ch);
        can->Write(can_ch, send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::READY_TO_SWITCH_ON) {
            break;
        }
    }

    if(current_status != Servo_Status::READY_TO_SWITCH_ON) {
        std::cout << "Servo motor enable failed at SWITCH_ON_DISABLED to READY_TO_SWITCH_ON." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is READY_TO_SWITCH_ON, continue..." << std::endl;
    }

    send_frame.Data[0] = 0x27;

    for(int i = 0; i < 10; i++)
    {
        can->Write(can_ch,send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::SWITCHED_ON) {
            break;
        }
    } 
    
    if(current_status != Servo_Status::SWITCHED_ON) {
        std::cout << "Servo motor enable failed at READY_TO_SWITCH_ON to SWITCHED_ON." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is SWITCHED_ON, continue..." << std::endl;
    }

    for(int i = 0; i < 10; i++) {
        send_frame.Data[0] = 0x2F;
        can->Write(can_ch,send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::OPERATION_ENABLED) {
            break;
        }
    }

    if(current_status != Servo_Status::OPERATION_ENABLED) {
        std::cout << "Servo motor enable failed at SWITCHED_ON to OPERATION_ENABLED." << std::endl;
        return -1;
    }
    else {
        std::cout << "Current status is OPERATION_ENABLED, enable completed." << std::endl;
    }

    return 0;
}

int ZeroErr::SendDisableMotorPDO()
{
    can->clr_send_buffer(can_ch);

    CanStamped send_frame;
    send_frame.ID = 0x201;
    send_frame.DataLen = 8;
    send_frame.Data[0] = 0x06;
    send_frame.Data[1] = 0x00;
    send_frame.Data[2] = 0x00;
    send_frame.Data[3] = 0x00;
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;

    for(int i = 0; i < 10; i++)
    {
        can->clr_send_buffer(can_ch);
        can->Write(can_ch,send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::READY_TO_SWITCH_ON) {
            break;
        }
    }

    if(current_status != Servo_Status::READY_TO_SWITCH_ON) {
        std::cout << "Servo motor disabled failed at OPERATION_ENABLED to READY_TO_SWITCH_ON." << std::endl;
        return -1;
    }

    send_frame.Data[0] = 0x00;
    for(int i = 0; i < 10; i++)
    {
        can->Write(can_ch,send_frame);
        can->send_msg(can_ch);
        usleep(500000);
        SendSyncFrame();
        can->recv_msg(can_ch);
        if(current_status == Servo_Status::SWITCH_ON_DISABLED) {
            break;
        }
    } 

    if(current_status != Servo_Status::SWITCH_ON_DISABLED) {
        std::cout << "Servo motor disabled failed at READY_TO_SWITCH_ON to SWITCH_ON_DISABLED." << std::endl;
        return -1;
    }
    return 0;
}

void ZeroErr::SendStopRemoteNode()
{
    can->clr_send_buffer(1);
    CanStamped setup_frame;
    // COB-ID 0x00,NMT frame
    // ctrl word:0x01 start remote node
    // ctrl word:0x02 close remote node
    setup_frame.ID = 0x00;
    setup_frame.DataLen = 2;
    setup_frame.Data[0] = 0x02;
    setup_frame.Data[1] = slave_id;

    std::cout << "ctrl word:0x02 stop remote node" << std::endl;
    can->Write(can_ch,setup_frame);
    can->Update(can_ch);
}

void ZeroErr::SendResetComm()
{
    can->clr_send_buffer(can_ch);
    CanStamped setup_frame;
    // ctrl word:0x82 reset communication
    setup_frame.ID = 0x00;
    setup_frame.DataLen = 2;
    std::cout << "ctrl word:0x82 reset communication" << std::endl;
    setup_frame.Data[0] = 0x82;
    setup_frame.Data[1] = 0x00 + slave_id;
    can->Write(can_ch, setup_frame);
    can->Update(can_ch);
}

void ZeroErr::SendStartRemoteNode()
{
    can->clr_send_buffer(can_ch);
    CanStamped setup_frame;
    // start node
    setup_frame.ID = 0x00;
    setup_frame.DataLen = 2;
    setup_frame.Data[0] = 0x01;
    setup_frame.Data[1] = 0x00 + slave_id;
    can->Write(can_ch,setup_frame);
    can->Update(can_ch);
}

void ZeroErr::Setup()
{
    CanStamped setup_frame;
    SendStopRemoteNode();

    SendResetComm();

    // int result = 0;
    // // set profile position mode
    // result = can->COE().write<uint8_t>(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0, 1);

    // if(result != 0) {
    //     std::cout << "Setup profile position mode failed." << std::endl;
    //     return;
    // } 
    SendSetWorkModeSDO(Servo_Mode::POS_MODE);

    int8_t mode = 0;
    bool is_acq = false;
    SendAcquireValidWorkModeSDO(mode, is_acq);
    if(is_acq && (mode == 1))
    {
        std::cout << "Setup profile position mode successfully." << std::endl;
    }
    else 
    {
        std::cout << "Setup profile position mode failed." << std::endl;
        return;
    }

    SendSetProfiledVelocitySDO(profiled_vel);
    SendSetProfiledAccSDO(profiled_acc);
    SendSetProfiledDecSDO(profiled_dec);

    // disable sync
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1005, 0, 0x80);
    // set communication cycle period (1000us)
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1006, 0, 0x03E8);

    can->COE().write<uint32_t>(can_ch, slave_id, 0x1800, 0x1, 0x80000181);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1800, 0x2, 0x1);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1A00, 0x0, 0x0);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 0x1, 0x60410010);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1A00, 0x2, 0x60640020);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1A00, 0, 0x2);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1800, 1, 0x0181);

    can->COE().write<uint32_t>(can_ch, slave_id, 0x1400, 0x1, 0x80000201);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1400, 0x2, 0x1);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1600, 0, 0);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1600, 0x1, 0x60400010);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1600, 0x2, 0x607A0020);
    can->COE().write<uint8_t>(can_ch, slave_id, 0x1600, 0, 0x2);
    can->COE().write<uint32_t>(can_ch, slave_id, 0x1400, 0x1, 0x0201);

    SendStartRemoteNode();

    int32_t pos = 0;
    is_acq = false;
    SendAcquireActualPosSDO(pos, is_acq);

    SendSyncFrame();
}

int32_t ZeroErr::SendSetTargetPosSDO(float degree)
{
    uint32_t set_val = static_cast<uint32_t> (degree * resolution / 360);
    std::cout << "set pos is:" << std::dec <<set_val << std::endl;
    // set_val = 0;
    
    can->COE().write<uint32_t>(can_ch, slave_id, TARGET_POS_INDEX, 0, set_val); // set target position
    usleep(20000);
    return 0;
}

bool ZeroErr::SendSetWorkModeSDO(Servo_Mode mode)
{
    bool is_set_ok = false;
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x0F); // Enable Operation
    usleep(10000);
    uint16_t ctrl = 0;
    bool is_acq = false;
    SendAcquireCtrlWordSDO(ctrl, is_acq);
    if (is_acq != true) {
        std::cout << "Set ctrl word to 0xF failed." << std::endl;
        return false;
    }
    
    if(ctrl != 0x0F) {
        std::cout << "Set ctrl word to 0xF failed." << std::endl;
        return false;
    }

    can->COE().write<int8_t>(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0, static_cast<int8_t>(mode));
    usleep(10000);
    
    int8_t read_mode = 0;
    is_acq = false;
    SendAcquireServoWorkModeSDO(read_mode, is_acq);
    if (is_acq != true) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }
    if (read_mode != static_cast<int8_t>(mode)) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }

    read_mode = 0;
    is_acq = false;
    SendAcquireValidWorkModeSDO(read_mode, is_acq);
    if (is_acq != true) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }
    if (read_mode != static_cast<int8_t>(mode)) {
        std::cout << "Set work mode failed." << std::endl;
        return false;
    }

    if (read_mode != static_cast<int8_t>(Servo_Mode::POS_MODE)) {
        is_pos_mode = false;
    }

    return true;
}

bool ZeroErr::SendSetTargetVelocitySDO(int32_t rpm)
{
    bool is_set_ok = false;
    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);
    std::cout << "set velocity:" << std::hex << set_val << std::endl;
    can->COE().write<uint32_t>(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0, set_val); 
    usleep(10000);

    int32_t target_vel = 0;
    bool is_acq = false;
    SendAcquireTargetVelocitySDO(target_vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set target velocity failed." << std::endl;
        return false;
    }
    
    if(target_vel != set_val) {
        std::cout << "Set target velocity failed." << std::endl;
        return false;
    }

    return true;
}

bool ZeroErr::SendSetProfiledVelocitySDO(uint32_t deg_per_s)
{
    bool is_set_ok = false;
    uint32_t set_val = static_cast<uint32_t> (deg_per_s * 1456.36);
    std::cout << "Set profiled velocity is:" << set_val << std::endl;
    // set_val = 5566;
    // std::cout << "resolution:" << resolution << "set velocity:" << std::hex << set_val << std::endl;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_VELOCITY_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledVelSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled velocity failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled velocity failed." << std::endl;
        return false;
    }

    return true;
}

bool ZeroErr::SendSetProfiledVelocityPDO(uint32_t rpm)
{
    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x201 + slave_id;
    send_frame.DataLen = 8;

    send_frame.Data[0] = static_cast<uint8_t> (set_val & 0xFF);
    send_frame.Data[1] = static_cast<uint8_t> ((set_val >> 8) & 0xFF);
    send_frame.Data[2] = static_cast<uint8_t> ((set_val >> 16) & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t> ((set_val >> 24) & 0xFF);
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
}

bool ZeroErr::SendSetProfiledAccSDO(uint32_t deg_per_ss)
{
    bool is_set_ok = false;
    uint32_t set_val = static_cast<uint32_t> (deg_per_ss * 1456.36);
    // set_val = 5566;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_ACC_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledAccSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled acceleration failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled acceleration failed." << std::endl;
        return false;
    }

    return true;
}

bool ZeroErr::SendSetProfiledDecSDO(uint32_t deg_per_ss)
{
    bool is_set_ok = false;
    uint32_t set_val = static_cast<uint32_t> (deg_per_ss * 1456.36);
    // set_val = 5566;
    can->COE().write<uint32_t>(can_ch, slave_id, PROFILED_DEC_INDEX,  0, set_val); 
    usleep(10000);

    uint32_t vel = 0;
    bool is_acq = false;
    SendAcquireProfiledDecSDO(vel, is_acq);
    if (is_acq != true) {
        std::cout << "Set profiled deceleration failed." << std::endl;
        return false;
    }
    
    if(vel != set_val) {
        std::cout << "Set profiled deceleration failed." << std::endl;
        return false;
    }

    return true;
}

void ZeroErr::SendSetCWordShutDown()
{
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x6);
    usleep(10000);
}

void ZeroErr::SendSetCWordSwitchOn()
{
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x7);
    usleep(10000);
}

void ZeroErr::SendSetCWordEnOperation()
{
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0xF);
    usleep(10000);
}

void ZeroErr::SendSetCWordClrFault()
{
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x80);
    usleep(10000);
}

void ZeroErr::SendSetCWordClrZero()
{
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x00);
    usleep(10000);
}

void ZeroErr::SendSetCWordCmdNonImmTriggered(float degree)
{
    // check last movement
    uint16_t status = 0;
    bool result = false;
    for(int i=0; i<50; i++)
    {
        std::cout << "Check status num:" << std::dec << i << std::endl;
        SendAcquireStatusWordSDO(status, result);
        if(result && !(status & 0x1000)) // bit12=0 => can receive
            break;
        usleep(10000);
    } 

    // Write target position
    SendSetTargetPosSDO(degree);
    usleep(5000);

    // non-immediate (bit5=0, bit4=1)
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x0F | 0x10);
    usleep(5000);

    // Wait for STATUS_WORD bit12=1
    bool received = false;
    for (int i = 0; i < 50; i++) 
    {
        SendAcquireStatusWordSDO(status, result);
        if (result && (status & 0x1000)) // bit12 = 1
        {
            received = true;
            break;
        }
        usleep(10000);
    }

    if (!received)
    {
        std::cout << "Non-immediate update: failed to send position command." << std::endl;
        return;
    }

    // clear bit4
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x0F);
    usleep(5000);
}

void ZeroErr::SendSetCWordCmdImmTriggered(float degree)
{
    // immediate (bit5=1, bit4=0)
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x2F);
    usleep(5000);
    
    // Write target position
    SendSetTargetPosSDO(degree);
    
    // Trigger (bit5=1, bit4=1)
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x3F);
    usleep(5000);

    // Wait for STATUS_WORD bit12=1
    bool result = false;
    uint16_t status = 0;
    bool received = false;
    for(int i = 0; i < 10; i++)
    {
        SendAcquireStatusWordSDO(status, result);
        if(result && (status & 0x1000)) {
            received = true;
            break;
        }
        usleep(5000);
    }

    // Clear bit4
    if (received == true)
    {
        can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x2F);
    }
    else
    {
        std::cout << "Immediate update pos failed." << std::endl;
    }
}

void ZeroErr::SendAcquireValidWorkModeSDO(int8_t &mode, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, VALID_WORK_MODE_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        mode = can->COE().get_valid_work_mode();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireServoWorkModeSDO(int8_t &mode, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_WORK_MODE_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        mode = can->COE().get_servo_work_mode();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireStatusWordSDO(uint16_t &status, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_STATUS_WORD_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        status = can->COE().get_current_status_word();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}


void ZeroErr::SendAcquireCtrlWordSDO(uint16_t &ctrl, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        ctrl = can->COE().get_current_ctrl_word();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireTargetPosSDO(int32_t &pos, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_POS_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        pos = can->COE().get_target_pos();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireActualPosSDO(int32_t &pos, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, ACTUAL_POS_INDEX_ZEROERR, 0);
    if (can->COE().get_parse_status() == true) {
        pos = can->COE().get_actual_pos();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireTargetVelocitySDO(int32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_target_velocity();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireActualVelocitySDO(int32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, TARGET_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_actual_velocity();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireProfiledVelSDO(uint32_t &vel, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_VELOCITY_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        vel = can->COE().get_profiled_vel();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireProfiledAccSDO(uint32_t &acc, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_ACC_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        acc = can->COE().get_profiled_acc();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireProfiledDecSDO(uint32_t &dec, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, PROFILED_DEC_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        dec = can->COE().get_profiled_dec();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendAcquireActualCurrentSDO(int16_t &current, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, ACTUAL_CURRENT_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        current = can->COE().get_actual_current();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendQuickStopSDO()
{
    uint16_t ctrl = 0;
    bool result = false;

    // read current CTRL_WORD
    SendAcquireCtrlWordSDO(ctrl, result);
    if(!result)
    {
        std::cout << "Failed to read control word!" << std::endl;
        return;
    }

    // clear bit2 of CTRL_WORD
    ctrl &= ~(1 << 2);

    // write CTRL_WORD
    can->COE().write<uint16_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, ctrl);
}

bool ZeroErr::SendResumeFromQuickStopSDO()
{
    uint16_t ctrl = 0;
    bool result = false;

    GetCurStatusPDO();
    // 1) 若 Fault/Fault reaction：做 Fault reset（bit7 上升沿）
    if(current_status == Servo_Status::FAULT || current_status == Servo_Status::FAULT_REACTION_ACT) {
        SendSetCWordClrFault();
        usleep(10000);
        SendSetCWordClrZero();
        usleep(10000);
        if(!WaitState(Servo_Status::SWITCH_ON_DISABLED)) {
            std::cout << "Fault reset failed" << std::endl;
            return false;
        }
    }

    // 2) 标准三步：Shutdown -> Switch on -> Enable operation
    //    这一步同时确保 quick stop 解除(bit2=1) 且 Halt(bit8=0)
    SendSetCWordShutDown(); // Shutdown
    if(!WaitState(Servo_Status::READY_TO_SWITCH_ON))
    {
        std::cout << "Wait ReadyToSwitchOn timeout" << std::endl; 
        return false;
    }

    SendSetCWordSwitchOn(); // Switch on
    if(!WaitState(Servo_Status::SWITCHED_ON))
    {
        std::cout << "Wait SwitchedOn timeout" << std::endl; 
        return false;
    }

    SendSetCWordEnOperation(); // Enable operation
    if(!WaitState(Servo_Status::OPERATION_ENABLED))
    {
        std::cout << "Wait OperationEnabled timeout" << std::endl; 
        return false;
    }

    std::cout << "Resume OK: Operation enabled" << std::endl;
    return true;
}   

void ZeroErr::SendReadQuickStopOptionSDO(uint16_t &option, bool &is_acq)
{
    can->COE().clr_parse_status();
    can->COE().read(can_ch, slave_id, STOP_OPTION_INDEX, 0);
    if (can->COE().get_parse_status() == true) {
        option = can->COE().get_stop_option();
        is_acq = true;
    }
    else {
        is_acq = false;
    }
}

void ZeroErr::SendWriteQuickStopOptionCode(int16_t option)
{
    can->COE().write<int16_t>(can_ch, slave_id, STOP_OPTION_INDEX,  0, option);

    std::cout << "Set Quick Stop Option Code = " << option << std::endl;
}

void ZeroErr::SendMoveTargetPosPDO(float degree)
{
    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return;
    }

    uint32_t set_val = static_cast<uint32_t> (degree * resolution / 360);

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x200 + slave_id;
    send_frame.DataLen = 8;

    // ctrl word
    send_frame.Data[0] = 0x2F;
    send_frame.Data[1] = 0x00;

    // transfer float pos to int32_t
    int32_t pos_int = static_cast<int32_t>(set_val);
    send_frame.Data[2] = static_cast<uint8_t>(pos_int & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t>((pos_int >> 8) & 0xFF);
    send_frame.Data[4] = static_cast<uint8_t>((pos_int >> 16) & 0xFF);
    send_frame.Data[5] = static_cast<uint8_t>((pos_int >> 24) & 0xFF);
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);

    can->clr_send_buffer(can_ch);
    send_frame.Data[0] = 0x3F;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);

    SendSyncFrame();
}

void ZeroErr::SendMoveTargetPosPDO(float pos, uint32_t rpm)
{
    if (is_pos_mode != true) {
        int8_t read_mode = 0;
        bool is_acq = false;
        for (int i = 0; i < 3; i++)
        {
            SendSetWorkModeSDO(Servo_Mode::POS_MODE);
            usleep(10000);
            SendAcquireValidWorkModeSDO(read_mode, is_acq);
            if((read_mode == static_cast<int8_t>(Servo_Mode::POS_MODE)) && is_acq == true) {
                is_pos_mode = true;
                break;
            }
        }
    }

    if(is_pos_mode != true) {
        std::cout << "Set servo mode to pos failed." << std::endl;
        return;
    }

    uint32_t set_val = rpm * (resolution * RPM_TO_DEC_MULT);

    can->clr_send_buffer(can_ch);
    CanStamped send_frame;
    send_frame.ID = 0x201 + slave_id;
    send_frame.DataLen = 8;

    send_frame.Data[0] = static_cast<uint8_t> (set_val & 0xFF);
    send_frame.Data[1] = static_cast<uint8_t> ((set_val >> 8) & 0xFF);
    send_frame.Data[2] = static_cast<uint8_t> ((set_val >> 16) & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t> ((set_val >> 24) & 0xFF);
    send_frame.Data[4] = 0x00;
    send_frame.Data[5] = 0x00;
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);

    send_frame.ID = 0x200 + slave_id;
    // send_frame.DataLen = 8;

    // ctrl word
    send_frame.Data[0] = 0x2F;
    send_frame.Data[1] = 0x00;

    // transfer float pos to int32_t
    int32_t pos_int = static_cast<int32_t>(pos);
    send_frame.Data[2] = static_cast<uint8_t>(pos_int & 0xFF);
    send_frame.Data[3] = static_cast<uint8_t>((pos_int >> 8) & 0xFF);
    send_frame.Data[4] = static_cast<uint8_t>((pos_int >> 16) & 0xFF);
    send_frame.Data[5] = static_cast<uint8_t>((pos_int >> 24) & 0xFF);
    send_frame.Data[6] = 0x00;
    send_frame.Data[7] = 0x00;
    can->Write(can_ch, send_frame);

    // ctrl word
    send_frame.Data[0] = 0x3F;
    send_frame.Data[1] = 0x00;

    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
    // usleep(500000);
}

void ZeroErr::parseStatusWord(uint16_t status)
{
    // std::cout << "Statusword: 0x" 
    //           << std::hex << std::setw(4) << std::setfill('0') << status 
    //           << std::dec << " (" << std::bitset<16>(status) << ")\n";

    // CiA 402 标准状态位
    // if (status & (1 << 0)) std::cout << " - Ready to Switch On\n";
    // if (status & (1 << 1)) std::cout << " - Switched On\n";
    // if (status & (1 << 2)) std::cout << " - Operation Enabled\n";
    // if (status & (1 << 3)) std::cout << " - Fault\n";
    // if (status & (1 << 4)) std::cout << " - Voltage Enabled\n";
    // if (status & (1 << 5)) std::cout << " - Quick Stop Active\n";
    // if (status & (1 << 6)) std::cout << " - Switch On Disabled\n";
    // if (status & (1 << 7)) std::cout << " - Warning\n";
    // if (status & (1 << 8)) std::cout << " - Manufacturer specific (bit8)\n";
    // if (status & (1 << 9)) std::cout << " - Remote (Set-point from network)\n";
    // if (status & (1 << 10)) std::cout << " - Target Reached\n";
    // if (status & (1 << 11)) std::cout << " - Internal Limit Active\n";
    // if (status & (1 << 12)) std::cout << " - Manufacturer specific (bit12)\n";
    // if (status & (1 << 13)) std::cout << " - Manufacturer specific (bit13)\n";
    // if (status & (1 << 14)) std::cout << " - Manufacturer specific (bit14)\n";
    // if (status & (1 << 15)) std::cout << " - Manufacturer specific (bit15)\n";

    // 简单判断状态机模式（参考 CiA 402）
    if ((status & 0x004F) == 0x0000) {
        std::cout << "State: Not ready to switch on\n";
    }
    else if ((status & 0x004F) == 0x0040) {
        std::cout << "State: Switch on disabled\n";
        current_status = Servo_Status::SWITCH_ON_DISABLED;
    }
    else if ((status & 0x006F) == 0x0021) {
        std::cout << "State: Ready to switch on\n";
        current_status = Servo_Status::READY_TO_SWITCH_ON;
    }
    else if ((status & 0x006F) == 0x0023) {
        std::cout << "State: Switched on\n";
        current_status = Servo_Status::SWITCHED_ON;
    }
    else if ((status & 0x006F) == 0x0027) {
        std::cout << "State: Operation enabled\n";
        current_status = Servo_Status::OPERATION_ENABLED;
    }
    else if ((status & 0x006F) == 0x0007) {
        std::cout << "State: Quick stop active\n";
        current_status = Servo_Status::QUICK_STOP_ACT;
    }
    else if ((status & 0x004F) == 0x000F) {
        std::cout << "State: Fault reaction active\n";
        current_status = Servo_Status::FAULT_REACTION_ACT;
    }
    else if ((status & 0x004F) == 0x0008) {
        std::cout << "State: Fault\n";
        current_status = Servo_Status::FAULT;
    }
}

int ZeroErr::processRecvTPDOFrame(const CanStamped &recv_frame)
{
    memcpy(&status_fbk, recv_frame.Data, 8);
    parseStatusWord(status_fbk.status);
    if(current_status == Servo_Status::FAULT) {
        std::cout << "Servo motor in FAULT status, disable servo motor." << std::endl;
        // SendDisableMotorPDO();
        can->clr_send_buffer(can_ch);

        can->COE().read(can_ch, slave_id, 0x1001, 0);

        // clear error bit
        can->COE().write<uint8_t>(can_ch, slave_id, SERVO_CTRL_WORD_INDEX, 0, 0x80);

        SendDisableMotorPDO();

        exit(-1);
    }
}

int ZeroErr::processRecvEmcyFrame(const CanStamped &recv_frame)
{
    std::cout << "Received EMCY frame." << std::endl;
    _error_code = recv_frame.Data[0] | (recv_frame.Data[1] << 8);
    uint8_t error_register = recv_frame.Data[2];
    uint8_t error_ext = recv_frame.Data[3]; // 假设扩展错误状态在 Data[3]

    std::cout << std::hex << "error code is:0x" <<_error_code 
                << " error register is:0x"<< static_cast<int>(error_register) <<std::endl;
     
    // auto it = emcy_error_map.find(_error_code);
    // if (it != emcy_error_map.end()) {
    //     std::cout << "EMCY Error: " << it->second << std::endl;
    // } else {
    //     std::cout << "EMCY Error: Unknown error code" << std::endl;
    // }

    // SendDisableMotorPDO();

    return 0;
}

void ZeroErr::SendSyncFrame()
{
    CanStamped send_frame;
    send_frame.ID = 0x80;
    send_frame.DataLen = 1;
    send_frame.Data[0] = 0x00;
    can->Write(can_ch, send_frame);
    can->send_msg(can_ch);
    usleep(10000);
    can->recv_msg(can_ch);
}

void ZeroErr::SendSetPosModeSDO()
{
    // can->COE().write<uint16_t>(slave_id, 0x6040, 0, 0x0F); // Enable Operation
    can->COE().write<uint8_t>(can_ch, slave_id, 0x6060, 0, 1);
}

int32_t ZeroErr::GetCurPosPDO()
{
    SendSyncFrame();
    return static_cast<float>(status_fbk.pos);
}

bool ZeroErr::WaitState(Servo_Status target_state, int tries, int delay)
{
    for(int i = 0; i < tries; i++)
    {
        GetCurStatusPDO();
        if (current_status == target_state) {
            return true;
        }
        usleep(delay);
    }
    return false;
}

Servo_Status ZeroErr::GetCurStatusPDO()
{
    SendSyncFrame();
    usleep(10000);
    return current_status;
}

int16_t ZeroErr::GetCurrentPDO(float &ap, float &rms)
{
    SendSyncFrame();
    usleep(10000);
    
    ap = static_cast<float> (status_fbk.current / DEC_TO_AP_DIV);
    rms = static_cast<float> (status_fbk.current / DEC_TO_RMS_DIV);
    return status_fbk.current;
}

MotorStatusFbk ZeroErr::GetCurFBKPDO(float &pos, float &ap, float &rms)
{
    SendSyncFrame();
    usleep(10000);
    // std::cout << "status_fbk.current:" << status_fbk.current << std::endl;

    pos = static_cast<float>(status_fbk.pos);
    ap = static_cast<float>(status_fbk.current / DEC_TO_AP_DIV);
    rms = static_cast<float>(status_fbk.current / DEC_TO_RMS_DIV);
    return status_fbk;
}
